/*
 * Parser for command line parameters
 */
#include <boost/program_options.hpp>
// #include <getopt.h>

/*
 * C++ Standard Library headers
 */
#include <iostream>
#include <fstream>
#include <string>
#include <assert.h>
#include <exception>
#include <signal.h>
#include <chrono>
#include <regex>

#include <Poco/BinaryWriter.h>

#include <pugixml.hpp>

/*
 * MessageStructs and BinaryClient Objects
 */
#include "LLSInterface/BinaryInterfaceServer.h"
#include "LLSInterface/BinaryInterfaceDummyLaserData.h"

#include "hins/xingsong_driver.h"
#include "sending_interface.hpp"

using namespace hins;
using namespace std;
namespace po = boost::program_options;

Poco::Buffer<char> ConvertLaserMsg2Datagram(const ScanData &data, const float &start_angle, const float &end_angle, const std::string &frame_name);
static float ClampRange(float r, float min, float max);
// void ParamInit(ConnectionAddress &laser_conn_info, XingSongLaserParam &laser_param, ShadowsFilterParam &shadows_filter_param);
void ParamInit(XingSongLaserParam &laser_param, ShadowsFilterParam &shadows_filter_param);

// Signal handler function
void signal_handler(int signal)
{
	if (signal == SIGINT)
	{
		cout << "Ctrl+C signal received. Cleaning up resources and terminating program." << endl;
		// Add code to clean up resources here.
		// TODO. Now it does nothing beyond default signal handling for SIGINT.

		exit(0);
	}
}

bool validateIPAddress(const std::string &ipAddress)
{
	// match an IP address in the format of "xxx.xxx.xxx.xxx"
	// const std::regex pattern("^([0-9]{1,3}\\.){3}[0-9]{1,3}$");

	// a stricter regular expression for matching IPv4 addresses
	const std::regex pattern("^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$");
	// const std::regex pattern("^(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$");
	return std::regex_match(ipAddress, pattern);
}

bool handle_xml(std::string &config_file)
{
	// Read XML file
	pugi::xml_document doc;
	std::ifstream fin(config_file);
	pugi::xml_parse_result result = doc.load(fin);

	if (!result)
	{
		std::cerr << "Error parsing XML: " << result.description() << std::endl;
		return false;
	}

	// Access data
	pugi::xml_node root = doc.child("root");
	std::string name = root.child_value("name");
	int age = root.child("age").text().as_int();
	std::vector<std::string> hobbies;
	for (pugi::xml_node hobby : root.child("hobbies").children("hobby"))
	{
		hobbies.push_back(hobby.text().get());
	}

	// Modify data
	age++;
	root.child("age").text().set(age);
	pugi::xml_node hobby = root.child("hobbies").append_child("hobby");
	hobby.text().set("reading");

	// Write XML file
	std::ofstream fout("example_out.xml");
	doc.save(fout, "  ");
	return true;
}

int main(int argc, char **argv)
{
	/* =================================
	 * Command line parsing
	 */
	std::string lidar_ip;
	int lidar_port;
	int simple_port;
	std::string config_file;

	bool usage = false;

	po::options_description desc("Allowed options");
	desc.add_options()("help,h", "produce help message")("default,d", "use default option values");
	desc.add_options()("lidar_ip,f", po::value<std::string>()->default_value("192.168.1.88"), "LiDAR IP");
	desc.add_options()("lidar_port,F", po::value<int>()->default_value(8080), "LiDAR port");
	desc.add_options()("simple_port,T", po::value<int>()->default_value(4242), "port to which messages in simple protocol are sent");
	// desc.add_options()("config_file,c", po::value<std::string>()->default_value("config.xml"), "full path to configuration file");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (argc < 2 || vm.count("help"))
	{
		std::cout << "Usage: " << argv[0] << " [options]\n";
		std::cout << desc << "\n";
		std::cout << "Example 1: To transfer laser data from LiDAR with address 192.168.8.150 and port 5555 to port 9090\n\t"
				  << argv[0] << " -f 192.168.8.150 -F 5555 -T 9090\n";
		std::cout << "Example 2: To transfer laser data from LiDAR with default address 192.168.1.88 and default port 8080 to port 9090\n\t"
				  << argv[0] << " -T 9090\n";
		std::cout << "Example 3: To transfer laser data with all default option values, from LiDAR 192.168.1.88:8080 to port 4242\n\t"
				  << argv[0] << " -d\n";
		return 0;
	}

	// TODO. Not work as expected.
	// std::cout << "vm size: " << vm.size() << "\n";
	// if (vm.empty())
	// {
	// 	std::cout << "No options specified\n";
	// 	std::cout << "Usage: " << argv[0] << " [options]\n";
	// 	std::cout << desc << "\n";
	// 	return 1;
	// }

	if (vm.count("default"))
	{
		lidar_ip = vm["lidar_ip"].as<std::string>();
		lidar_port = vm["lidar_port"].as<int>();
		simple_port = vm["simple_port"].as<int>();
		// config_file = vm["config_file"].as<std::string>();
	}
	else
	{
		if (vm.count("lidar_ip"))
		{
			if (not validateIPAddress(lidar_ip = vm["lidar_ip"].as<std::string>()))
			{
				std::cout << "Invalid IP address format of LiDAR: " << lidar_ip << std::endl;
				return 1;
			}
		}

		if (vm.count("lidar_port"))
		{
			lidar_port = vm["lidar_port"].as<int>();
		}

		if (vm.count("simple_port"))
		{
			simple_port = vm["simple_port"].as<int>();
		}

		if (vm.count("config_file"))
		{
			config_file = vm["config_file"].as<std::string>();
		}
	}
	std::cout << "LiDAR IP: " << lidar_ip << "\n";
	std::cout << "LiDAR port: " << lidar_port << "\n";
	std::cout << "Simple port: " << simple_port << "\n";
	std::cout << "Config file: " << config_file << "\n";

	float start_angle, end_angle;
	start_angle = 0.;
	end_angle = 2 * M_PI;

	std::string topic_name, frame_name;
	topic_name = std::string("scan1");
	frame_name = "laser1";

	// 2. 雷达驱动
	ConnectionAddress laser_conn_info(lidar_ip, lidar_port);
	// laser_conn_info.SetAddress(lidar_ip);
	// laser_conn_info.SetPort(lidar_port);

	XingSongLaserParam laser_param;
	laser_param.run_state = "run";

	ShadowsFilterParam shadows_filter_param;

	// 初始化雷达参数
	// ParamInit(laser_conn_info, laser_param, shadows_filter_param);
	ParamInit(laser_param, shadows_filter_param);

	// 启动雷达
	XingSongDriverHdr driver_hdr = std::make_shared<XingSongDriver>(laser_conn_info, laser_param, shadows_filter_param);

	// Ignore sigpipe
	signal(SIGPIPE, SIG_IGN);
	signal(SIGINT, signal_handler);

	/* =================================
	 * Open server and start sending data
	 */
	try
	{
		BinaryInterfaceServer server(simple_port);

		while (true)
		{
			ScanData data = driver_hdr->GetFullScan();
			Poco::Buffer<char> laser_datagram = ConvertLaserMsg2Datagram(data, start_angle, end_angle, frame_name);
			server.write(laser_datagram.begin(), laser_datagram.size());
			sleep(1 / 60);
		}
	}
	catch (std::exception &ex)
	{
		std::cerr << "TestClientLocalizationPose died with error!" << std::endl;
		std::cerr << "What: " << std::string(ex.what()) << std::endl;
	}

	return 0;
}

uint16_t current_scan_num{0};
uint64_t unique_id{0};
Poco::Buffer<char> ConvertLaserMsg2Datagram(const ScanData &data, const float &start_angle, const float &end_angle, const std::string &frame_name)
{
	// convert the ROS message to a locator ClientSensorLaserDatagram
	const size_t resulting_msg_size = 2									// scanNum
									  + 5 * 8							// time_start, uniqueId, duration_beam, duration_scan, duration_rotate
									  + 6 * 4							// numBeams, angleStart, angleEnd, angleInc, minRange, maxRange
									  + 4								// ranges->length
									  + data.distance_data.size() * 4	// ranges->elements
									  + 1								// hasIntensities
									  + 2 * 4							// minIntensity, maxIntensity
									  + 4								// intensities->length
									  + data.amplitude_data.size() * 4; // intensities->elements

	Poco::Buffer<char> buffer(resulting_msg_size);
	Poco::MemoryBinaryWriter writer(buffer, Poco::BinaryWriter::StreamByteOrder::LITTLE_ENDIAN_BYTE_ORDER);

	// scanNum
	writer << static_cast<uint16_t>(++current_scan_num);
	// time_start
	writer << (std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch())).count();
	// uniqueId
	writer << static_cast<uint64_t>(++unique_id);
	// duration_beam
	// double duration_beam = (float(data.time_increment) / 1000000) / (data.distance_data.size() - 1);
	double duration_beam = (float(data.time_increment) / 1000000) / data.distance_data.size();
	// static_cast<double>(data.time_increment != 0.0f ? fabs(data.time_increment) : (data.angle_max - msg.angle_min) * (scan_time != 0.0f ? scan_time : msg.scan_time) / (2.0 * M_PI * (msg.ranges.size() - 1)));
	writer << duration_beam;
	// duration_scan
	// double duration_scan = duration_beam * static_cast<double>(data.distance_data.size() - 1);
	double duration_scan = duration_beam * static_cast<double>(data.distance_data.size());
	writer << duration_scan;
	// duration_rotate (has to be > 0 for motion correction of scans)
	writer << duration_scan;
	// numBeams
	writer << static_cast<uint32_t>(data.distance_data.size());
	// angleStart
	writer << static_cast<float>(0.0);
	// angleEnd
	writer << static_cast<float>(2 * M_PI);
	// angleInc
	writer << static_cast<float>(2 * M_PI / (data.distance_data.size() - 1));
	// writer << static_cast<float>(2 * M_PI / data.distance_data.size()); // result in message error and connection failure
	// minRange
	writer << static_cast<float>(0.05);
	// maxRange
	writer << static_cast<float>(60.0);
	// ranges.length
	writer << static_cast<uint32_t>(data.distance_data.size());
	// ranges.elements
	// distance unit mm to m
	for (const auto r : data.distance_data)
	{
		writer << static_cast<float>(std::isnan(r) ? -1e4f : ClampRange(r / 1000.0, -1e4f, 1e4f));
	}
	// hasIntensities
	writer << static_cast<char>(!data.amplitude_data.empty());
	// FIXME intensities ranges not in sensor_msgs/LaserScan. Where to get from?
	// minIntensity
	writer << static_cast<float>(0.f);
	// maxIntensity
	writer << static_cast<float>(1.f);
	// intensitites.length
	writer << static_cast<uint32_t>(data.amplitude_data.size());
	// intensities.elements
	for (const auto intensity : data.amplitude_data)
	{
		writer << static_cast<float>(intensity / 1000.0);
	}
	writer.flush();

	if (resulting_msg_size != buffer.size())
		std::cerr << "ConvertLaserMsg2DataGram: message size mismatch!" << std::endl;

	return buffer;
}

/// clamp scan data to specified range
static float ClampRange(float r, float min, float max)
{
	return std::min({std::max({r, min}), max});
};

// void ParamInit(ConnectionAddress &laser_conn_info, XingSongLaserParam &laser_param, ShadowsFilterParam &shadows_filter_param)
void ParamInit(XingSongLaserParam &laser_param, ShadowsFilterParam &shadows_filter_param)
{
	// std::string server_address;
	// int port;
	// server_address = std::string("192.168.1.88");
	// port = 8080;
	// laser_conn_info.SetAddress(server_address);
	// laser_conn_info.SetPort(port);

	int shadows_filter_neighbors, shadows_filter_window, shadows_filter_level, shadows_traverse_step;
	std::string measure_frequency, spin_frequency, sampling_size_per_position, noise_filter_level;
	float start_angle, end_angle, shadows_filter_max_angle, shadows_filter_min_angle;

	measure_frequency = std::string("200");
	spin_frequency = std::string("30");
	sampling_size_per_position = std::string("1");
	noise_filter_level = std::string("1");
	shadows_filter_max_angle = float(175.0);
	shadows_filter_min_angle = float(5.0);
	shadows_filter_neighbors = 1;
	shadows_filter_window = 2;
	shadows_traverse_step = 1;
	shadows_filter_level = -1;

	laser_param.measure_frequency_kHz = measure_frequency;
	laser_param.spin_frequency_Hz = spin_frequency;
	laser_param.sampling_size_per_position = sampling_size_per_position;
	laser_param.noise_filter_level = noise_filter_level;

	shadows_filter_param.max_angle = shadows_filter_max_angle;
	shadows_filter_param.min_angle = shadows_filter_min_angle;
	shadows_filter_param.neighbors = shadows_filter_neighbors;
	shadows_filter_param.window = shadows_filter_window;
	shadows_filter_param.traverse_step = shadows_traverse_step;
	shadows_filter_param.shadows_filter_level = shadows_filter_level;
}
